package com.spica.spicaschool.service;

import com.amap.api.maps.AMapUtils;
import com.amap.api.maps.model.LatLng;

import java.util.ArrayList;
import java.util.List;

/**
 * 轨迹优化工具类
 * <p>
 * 使用方法：
 * <p>
 * PathSmoothTool pathSmoothTool = new PathSmoothTool();
 * pathSmoothTool.setIntensity(2);//设置滤波强度，默认3
 * List<LatLng> mList = LatpathSmoothTool.kalmanFilterPath(list);
 */

public class PathSmoothTool {

  private int mIntensity = 3;
  private float mThreshhold = 1.0f;
  private float mNoiseThreshhold = 10;

  public PathSmoothTool() {

  }

  public int getIntensity() {
    return mIntensity;
  }

  public void setIntensity(int mIntensity) {
    this.mIntensity = mIntensity;
  }

  public float getThreshhold() {
    return mThreshhold;
  }

  public void setThreshhold(float mThreshhold) {
    this.mThreshhold = mThreshhold;
  }

  public void setNoiseThreshhold(float mnoiseThreshhold) {
    this.mNoiseThreshhold = mnoiseThreshhold;
  }

  /**
   * 轨迹平滑优化
   *
   * @param originlist 原始轨迹list,list.size大于2
   * @return 优化后轨迹list
   */
  public List<LatLng> pathOptimize(List<LatLng> originlist) {
    synchronized (this) {
      List<LatLng> list = removeNoisePoint(originlist);//去噪
      List<LatLng> afterList = kalmanFilterPath(list, mIntensity);//滤波
      return reducerVerticalThreshold(afterList, mThreshhold);//抽稀
    }
  }

  /**
   * 轨迹线路滤波
   *
   * @param originlist 原始轨迹list,list.size大于2
   * @return 滤波处理后的轨迹list
   */
  public List<LatLng> kalmanFilterPath(List<LatLng> originlist) {
    return kalmanFilterPath(originlist, mIntensity);
  }

  /**
   * 轨迹去噪，删除垂距大于20m的点
   *
   * @param originlist 原始轨迹list,list.size大于2
   * @return
   */
  public List<LatLng> removeNoisePoint(List<LatLng> originlist) {
    return reduceNoisePoint(originlist, mNoiseThreshhold);
  }

  /**
   * 单点滤波
   *
   * @param lastLoc 上次定位点坐标
   * @param curLoc 本次定位点坐标
   * @return 滤波后本次定位点坐标值
   */
  public LatLng kalmanFilterPoint(LatLng lastLoc, LatLng curLoc) {
    return kalmanFilterPoint(lastLoc, curLoc, mIntensity);
  }

  /**
   * 轨迹抽稀
   *
   * @param inPoints 待抽稀的轨迹list，至少包含两个点，删除垂距小于mThreshhold的点
   * @return 抽稀后的轨迹list
   */
  public List<LatLng> reducerVerticalThreshold(List<LatLng> inPoints) {
    return reducerVerticalThreshold(inPoints, mThreshhold);
  }

  /********************************************************************************************************/
  /**
   * 轨迹线路滤波
   *
   * @param originlist 原始轨迹list,list.size大于2
   * @param intensity 滤波强度（1—5）
   * @return
   */
  private List<LatLng> kalmanFilterPath(List<LatLng> originlist, int intensity) {
    synchronized (this) {
      List<LatLng> kalmanFilterList = new ArrayList<>();
      if (originlist == null || originlist.size() <= 2) { return kalmanFilterList; }
      initial();//初始化滤波参数
      LatLng latLng;
      LatLng lastLoc = originlist.get(0);
      kalmanFilterList.add(lastLoc);
      for (int i = 1; i < originlist.size(); i++) {
        LatLng curLoc = originlist.get(i);
        latLng = kalmanFilterPoint(lastLoc, curLoc, intensity);
        if (latLng != null) {
          kalmanFilterList.add(latLng);
          lastLoc = latLng;
        }
      }
      return kalmanFilterList;
    }
  }

  /**
   * 单点滤波
   *
   * @param lastLoc 上次定位点坐标
   * @param curLoc 本次定位点坐标
   * @param intensity 滤波强度（1—5）
   * @return 滤波后本次定位点坐标值
   */
  private LatLng kalmanFilterPoint(LatLng lastLoc, LatLng curLoc, int intensity) {
    if (pdeltX == 0 || pdeltY == 0) {
      initial();
    }
    LatLng kalmanLatlng = null;
    if (lastLoc == null || curLoc == null) {
      return kalmanLatlng;
    }
    if (intensity < 1) {
      intensity = 1;
    } else if (intensity > 5) {
      intensity = 5;
    }
    for (int j = 0; j < intensity; j++) {
      kalmanLatlng = kalmanFilter(lastLoc.longitude, curLoc.longitude, lastLoc.latitude, curLoc.latitude);
      curLoc = kalmanLatlng;
    }
    return kalmanLatlng;
  }

  /***************************卡尔曼滤波开始********************************/
  private double lastLocationX; //上次位置
  private double currentLocationX;//这次位置
  private double lastLocationY; //上次位置
  private double currentLocationY;//这次位置
  private double estimateX; //修正后数据
  private double estimateY; //修正后数据
  private double pdeltX; //自预估偏差
  private double pdeltY; //自预估偏差
  private double mdeltX; //上次模型偏差
  private double mdeltY; //上次模型偏差
  private double gaussX; //高斯噪音偏差
  private double gaussY; //高斯噪音偏差
  private double kalmanGainX; //卡尔曼增益
  private double kalmanGainY; //卡尔曼增益

  private double m_R = 0;
  private double m_Q = 0;

  //初始模型
  private void initial() {
    pdeltX = 0.001;
    pdeltY = 0.001;
    //        mdelt_x = 0;
    //        mdelt_y = 0;
    mdeltX = 5.698402909980532E-4;
    mdeltY = 5.698402909980532E-4;
  }

  private LatLng kalmanFilter(double oldValueX, double valueX,
                              double oldValueY, double valueY) {
    lastLocationX = oldValueX;
    currentLocationX = valueX;
    gaussX = Math.sqrt(pdeltX * pdeltX + mdeltX * mdeltX) + m_Q;//计算高斯噪音偏差
    kalmanGainX = Math.sqrt((gaussX * gaussX) / (gaussX * gaussX
        + pdeltX * pdeltX)) + m_R; //计算卡尔曼增益
    estimateX = kalmanGainX * (currentLocationX - lastLocationX) + lastLocationX;//修正定位点
    mdeltX = Math.sqrt((1 - kalmanGainX) * gaussX * gaussX);//修正模型偏差
    lastLocationY = oldValueY;
    currentLocationY = valueY;
    gaussY = Math.sqrt(pdeltY * pdeltY + mdeltY * mdeltY) + m_Q;//计算高斯噪音偏差
    kalmanGainY = Math.sqrt((gaussY * gaussY) / (gaussY * gaussY
        + pdeltY * pdeltY)) + m_R; //计算卡尔曼增益
    estimateY = kalmanGainY * (currentLocationY - lastLocationY)
        + lastLocationY;    //修正定位点
    mdeltY = Math.sqrt((1 - kalmanGainY) * gaussY * gaussY);//修正模型偏差
    LatLng latlng = new LatLng(estimateY, estimateX);
    return latlng;
  }
  /***************************卡尔曼滤波结束**********************************/

  /***************************抽稀算法*************************************/
  private List<LatLng> reducerVerticalThreshold(List<LatLng> inPoints, float threshHold) {
    synchronized (this) {
      if (inPoints == null) {
        return null;
      }
      if (inPoints.size() <= 2) {
        return inPoints;
      }
      List<LatLng> ret = new ArrayList<LatLng>();
      for (int i = 0; i < inPoints.size(); i++) {
        LatLng pre = getLastLocation(ret);
        LatLng cur = inPoints.get(i);
        if (pre == null || i == inPoints.size() - 1) {
          ret.add(cur);
          continue;
        }
        LatLng next = inPoints.get(i + 1);
        double distance = calculateDistanceFromPoint(cur, pre, next);
        if (distance > threshHold) {
          ret.add(cur);
        }
      }
      return ret;
    }
  }

  private static LatLng getLastLocation(List<LatLng> oneGraspList) {
    if (oneGraspList == null || oneGraspList.size() == 0) {
      return null;
    }
    int locListSize = oneGraspList.size();
    LatLng lastLocation = oneGraspList.get(locListSize - 1);
    return lastLocation;
  }

  /**
   * 计算当前点到线的垂线距离
   *
   * @param p 当前点
   * @param lineBegin 线的起点
   * @param lineEnd 线的终点
   */
  private static double calculateDistanceFromPoint(LatLng p, LatLng lineBegin,
                                                   LatLng lineEnd) {
    double A = p.longitude - lineBegin.longitude;
    double B = p.latitude - lineBegin.latitude;
    double C = lineEnd.longitude - lineBegin.longitude;
    double D = lineEnd.latitude - lineBegin.latitude;

    double dot = A * C + B * D;
    double len_sq = C * C + D * D;
    double param = dot / len_sq;

    double xx, yy;

    if (param < 0 || (lineBegin.longitude == lineEnd.longitude
        && lineBegin.latitude == lineEnd.latitude)) {
      xx = lineBegin.longitude;
      yy = lineBegin.latitude;
      //            return -1;
    } else if (param > 1) {
      xx = lineEnd.longitude;
      yy = lineEnd.latitude;
      //            return -1;
    } else {
      xx = lineBegin.longitude + param * C;
      yy = lineBegin.latitude + param * D;
    }
    return AMapUtils.calculateLineDistance(p, new LatLng(yy, xx));
  }

  /***************************抽稀算法结束*********************************/

  private List<LatLng> reduceNoisePoint(List<LatLng> inPoints, float threshHold) {
    synchronized (this) {
      if (inPoints == null) {
        return null;
      }
      if (inPoints.size() <= 2) {
        return inPoints;
      }
      List<LatLng> ret = new ArrayList<LatLng>();
      for (int i = 0; i < inPoints.size(); i++) {
        LatLng pre = getLastLocation(ret);
        LatLng cur = inPoints.get(i);
        if (pre == null || i == inPoints.size() - 1) {
          ret.add(cur);
          continue;
        }
        LatLng next = inPoints.get(i + 1);
        double distance = calculateDistanceFromPoint(cur, pre, next);
        if (distance < threshHold) {
          ret.add(cur);
        }
      }
      return ret;
    }
  }

}
